#include "StreetNode.h"

#include <osg/Geometry>
#include <osg/ShapeDrawable>
#include <osgUtil/Tessellator>

#include <osg/Texture2D>
#include <osg/TexGen>
#include <osgDB/ReadFile>

#include <cassert>


#define HALFWIDTH 5.0f
#define HALFWIDTH2 10.0f
#define HALFWIDTH3 3.0f
#define HALFWIDTH4 7.0f
#define HALFWIDTH5 2.0f;

osg::Vec3 StreetNode::calculateNormal(const osg::Vec3& v1, const osg::Vec3& v2)
{
    // 2D normal
    osg::Vec3 subtract(v2 - v1); 
    float dx = subtract.x();
    float dy = subtract.y();
    osg::Vec3 normal(-dy, dx, 0.0f);
    normal.normalize();

    return normal;
}

osg::Vec3Array* StreetNode::constructPolyboard(osg::Vec3Array* points)
{
    osg::ref_ptr<osg::Vec3Array> vertices = new osg::Vec3Array;

    float thickness = m_width;

    for(int i = 0; i < points->size(); ++i)
    {
        if(i == 0)
        {
            // hack for degenerated lines
            if(points->size() < 2)
            {
                osg::Vec3 v1(points->at(i));
                osg::Vec3 v2(points->at(i)); 
                osg::Vec3 normal = calculateNormal(v1, v2);

                osg::Vec3 Pi = points->at(i);
                osg::Vec3 Gi(Pi + (normal * thickness));
                osg::Vec3 Hi(Pi - (normal * thickness));

                vertices->push_back(Gi);
                vertices->push_back(Hi);
            }
            else
            {
                osg::Vec3 v1(points->at(i));
                osg::Vec3 v2(points->at(i+1));
                osg::Vec3 normal = calculateNormal(v1, v2);

                osg::Vec3 Pi = points->at(i);
                osg::Vec3 Gi(Pi + (normal * thickness));
                osg::Vec3 Hi(Pi - (normal * thickness));

                vertices->push_back(Gi);
                vertices->push_back(Hi);
            }
            
        }
        else if(i == points->size()-1)
        {
            osg::Vec3 v1(points->at(i));
            osg::Vec3 v2(points->at(i-1));
            osg::Vec3 normal = calculateNormal(v1, v2);

            osg::Vec3 Pi = points->at(i);
            osg::Vec3 Gi(Pi - (normal * thickness));
            osg::Vec3 Hi(Pi + (normal * thickness));

            vertices->push_back(Gi);
            vertices->push_back(Hi);
        }
        else
        {
            osg::Vec3 v1(points->at(i));
            osg::Vec3 v2(points->at(i+1));
            osg::Vec3 v3(points->at(i-1));

            osg::Vec3 v2v1(v2 - v1);
            osg::Vec3 v3v1(v3 - v1);

            osg::Vec3 averageNormal = calculateNormal(v2v1, v3v1);
            averageNormal = averageNormal * osg::Matrix::scale(1.0f, 1.0f, 1.0f);

            osg::Vec3 Pi = points->at(i);
            osg::Vec3 Gi(Pi - (averageNormal * thickness));
            osg::Vec3 Hi(Pi + (averageNormal * thickness));

            vertices->push_back(Gi);
            vertices->push_back(Hi);
        }
    }

    return vertices.release();
}


StreetNode::StreetNode(osg::ref_ptr<osg::Vec3Array> verticies, QString name, unsigned int category):ToggleNode()
{
	styleStreet(category);
    m_name = name;

    // construct triangle trips from line strips
    osg::ref_ptr<osg::Vec3Array> triangleStrip = constructPolyboard(verticies.get());
    osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry;

    geometry->setVertexArray(triangleStrip);
    
    osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array();
    colors->push_back(m_color);
    geometry->setColorArray(colors.get());
    geometry->setColorBinding(osg::Geometry::BIND_OVERALL);

    
    osg::ref_ptr<osg::Vec3Array> normals = new osg::Vec3Array();
    normals->push_back(osg::Vec3(0.f, -1.f, 0.f));
    geometry->setNormalArray(normals.get());
    geometry->setNormalBinding(osg::Geometry::BIND_OVERALL);
    geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::TRIANGLE_STRIP, 0, triangleStrip->size()));

	this->addDrawable(geometry);
}

StreetNode::~StreetNode()
{

}

void StreetNode::styleStreet(unsigned int category)
{
	switch(category)
	{
		case 0: // secondary
			{
				m_color = osg::Vec4(255.f/255.f, 251.f/255.f, 137.f/255.f, 1.0);
				m_width = HALFWIDTH2;
				break;
			}

        case 1: // normal street
            {
                m_color = osg::Vec4(236.f/255.f, 152.f/255.f, 154.f/255.f, 1.0);
                m_width = HALFWIDTH;
                break;
            }
        case 2: // path
            {
                m_color = osg::Vec4(255.f/255.f, 255.f/255.f, 255.f/255.f, 1.0);
                m_width = HALFWIDTH3;
                break;
            }
        case 3: // primary
            {
                m_color = osg::Vec4(228/255.f, 109.f/255.f, 113.f/255.f, 1.0);
                m_width = HALFWIDTH2 * 1.5f;
                break;
            }

        case 4: // euclidean distance
            {
                m_color = osg::Vec4(0.0f, 0.0f, 0.0f, 1.0);
                m_width = HALFWIDTH4;
                break;
            }

		default:
			{
				m_color = osg::Vec4(255.f/255.f, 255.f/255.f, 255.f/255.f, 1.0);
				m_width = HALFWIDTH3;
				break;
			}
	}
}

QString StreetNode::getFeatureString()
{
    QString returnString("street name: ");
    returnString.append(m_name);
    return returnString;
}
